#include <stdio.h>

#include "modbus2645.h"
//#include "usart_app.h"
#include "mb.h"
#include "mbframe.h"
#include "mbproto.h"
#include "mbconfig.h"
#include "modbusdata.h"
#include "modbusfun.h"

//==================custom_def_start===============
//#include "mainevent.h"
//#include "mainapp.h"
//==================custom_def_end=================
UCHAR xMBUtilGetBits (UCHAR * ucByteBuf, USHORT usBitOffset, UCHAR ucNBits);

eMBErrorCode
eMBRegHoldingCB_Write (UCHAR * pucRegBuffer, USHORT usAddress,
		       USHORT usNRegs);

eMBErrorCode
eMBRegHoldingCB (UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs,
		 eMBRegisterMode eMode)
{
  eMBErrorCode eStatus = MB_ENOERR;

  USHORT REG_HOLDING_START;
//  USHORT  usRegHoldingStart;
  USHORT REG_HOLDING_NREGS;
  USHORT *usRegHoldingBuf = usRegHoldingBuf;

  if (((usAddress >= REG_HOLDING_START) &&
       (usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS))
      || (01))
    {
      //  iRegIndex = (int) (usAddress - usRegHoldingStart);
      switch (eMode)
	{
	  /* Pass current register values to the protocol stack. */
	case MB_REG_READ:

	  return eMBRegInputCB (pucRegBuffer, usAddress, usNRegs);
//        break;

	  /* Update current register values with new values from the
	   * protocol stack. */
	case MB_REG_WRITE:
	  return eMBRegHoldingCB_Write (pucRegBuffer, usAddress, usNRegs);
	  /*
	     while (usNRegs > 0)
	     {
	     usRegHoldingBuf[iRegIndex] = *pucRegBuffer++ << 8;
	     usRegHoldingBuf[iRegIndex] |= *pucRegBuffer++;
	     iRegIndex++;
	     usNRegs--;
	     }
	   */
	}
    }
  else
    {
      eStatus = MB_ENOREG;
    }
  return eStatus;

}

eMBErrorCode
eMBRegHoldingCB_Write (UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs)
{
  eMBErrorCode eStatus = MB_ENOERR;
  USHORT REG_INPUT_START;
  USHORT usRegInputStart;
  USHORT REG_INPUT_NREGS;
  struct STMODBUS2DATAP *stp;
  USHORT *usRegHoldingBuf;
  USHORT iRegIndex;
  stp = ModAddr2Search (usAddress);
  if (NULL == stp)
    {
      eStatus = MB_ENOREG;
      return eStatus;
    }
  REG_INPUT_START = (USHORT) stp->iMaddr;
  REG_INPUT_NREGS = (USHORT) stp->ilen;
  usRegInputStart = (USHORT) stp->iMaddr;
  usRegHoldingBuf = (USHORT *) stp->pData;

  if ((usAddress) == _InSide_FlashAccess_Addr_)
    {
      /* --opt
         McuFlashRandWrite ((unsigned long) _FlashFresh_Addr_,
         (char *) pucRegBuffer, usNRegs * sizeof (USHORT));
         _Flash_Addr_Set ((unsigned long) _FlashFresh_Addr_ +
         usNRegs * sizeof (USHORT));
       */
      return eStatus;
    }

  if ((usAddress >= REG_INPUT_START)
      && (usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS))
    {
      iRegIndex = (int) (usAddress - usRegInputStart);
      while (usNRegs > 0)
	{
	  usRegHoldingBuf[iRegIndex] = *pucRegBuffer++ << 8;
	  usRegHoldingBuf[iRegIndex] |= *pucRegBuffer++;
	  iRegIndex++;
	  usNRegs--;
	}
    }
  else
    {
      eStatus = MB_ENOREG;
    }

  return eStatus;
}

eMBErrorCode
eMBRegDiscreteCB (UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete)
{
  eMBErrorCode eStatus = MB_ENOERR;
  short iNDiscrete = (short) usNDiscrete;
  unsigned short usBitOffset;
  struct STMODBUS2DATAP *stp;
  USHORT REG_DISC_START;
  USHORT REG_DISC_SIZE;

  char *ucRegDiscBuf;
  /* Check if we have registers mapped at this block. */
  stp = ModAddr2Search (usAddress);
  if (NULL == stp)
    {
      eStatus = MB_ENOREG;
      return eStatus;
    }
  REG_DISC_START = (USHORT) stp->iMaddr + 1;
  REG_DISC_SIZE = (USHORT) stp->ilen * 16;	//USHORT ->16bIT

  ucRegDiscBuf = (char *) stp->pData;


  if ((usAddress >= REG_DISC_START) &&
      (usAddress + usNDiscrete <= REG_DISC_START + REG_DISC_SIZE))
    {
      usBitOffset = (unsigned short) (usAddress - REG_DISC_START);
      while (iNDiscrete > 0)
	{
	  *pucRegBuffer++ =
	    xMBUtilGetBits (ucRegDiscBuf, usBitOffset,
			    (unsigned char) (iNDiscrete >
					     8 ? 8 : iNDiscrete));
	  iNDiscrete -= 8;
	  usBitOffset += 8;
	}
    }
  else
    {
      eStatus = MB_ENOREG;
    }

  return eStatus;
}

eMBErrorCode
eMBRegCoilsCB (UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
	       eMBRegisterMode eMode)
{
  eMBErrorCode eStatus = MB_ENOERR;
  short iNCoils = (short) usNCoils;
  struct STMODBUS2DATAP *stp;
  unsigned short usBitOffset;
  USHORT REG_COILS_START;
  USHORT REG_COILS_SIZE;
  char *ucRegCoilsBuf;
  stp = ModAddr2Search (usAddress);
  if (NULL == stp)
    {
      eStatus = MB_ENOREG;
      return eStatus;
    }
  REG_COILS_START = (USHORT) stp->iMaddr + 1;
  REG_COILS_SIZE = (USHORT) stp->ilen * 16;	//USHORT ->16bIT
  ucRegCoilsBuf = (char *) stp->pData;
  /* Check if we have registers mapped at this block. */
  if ((usAddress >= REG_COILS_START) &&
      (usAddress + usNCoils <= REG_COILS_START + REG_COILS_SIZE))
    {
      usBitOffset = (unsigned short) (usAddress - REG_COILS_START);
      switch (eMode)
	{
	  /* Read current values and pass to protocol stack. */
	case MB_REG_READ:
	  while (iNCoils > 0)
	    {
	      *pucRegBuffer++ =
		xMBUtilGetBits (ucRegCoilsBuf, usBitOffset,
				(unsigned char) (iNCoils > 8 ? 8 : iNCoils));
	      iNCoils -= 8;
	      usBitOffset += 8;
	    }
	  break;

	  /* Update current register values. */
	case MB_REG_WRITE:
	  while (iNCoils > 0)
	    {
	      xMBUtilSetBits (ucRegCoilsBuf, usBitOffset,
			      (unsigned char) (iNCoils > 8 ? 8 : iNCoils),
			      *pucRegBuffer++);
	      iNCoils -= 8;
	    }
	  break;
	}

    }
  else
    {
      eStatus = MB_ENOREG;
    }
  return eStatus;
}

__weak void
ModbusKeepReg_Mod_Fun_Handle (void)
{
}
